#include "enc.h"

int CCONV ENC_AttachHandler(CPhidgetHandle ENC, void *userptr){

  int serialNo;
  CPhidget_DeviceID deviceID;
  int i, inputcount;
  
  CPhidget_getSerialNumber(ENC, &serialNo);
  
  //Retrieve the device ID and number of encoders so that we can set the enables if needed
  CPhidget_getDeviceID(ENC, &deviceID);
  CPhidgetEncoder_getEncoderCount((CPhidgetEncoderHandle)ENC, &inputcount);
  ROS_INFO("Encoder %10d attached! \n", serialNo);
  
  //the 1047 requires enabling of the encoder inputs, so enable them if this is a 1047    
  if (deviceID == PHIDID_ENCODER_HS_4ENCODER_4INPUT)
    {
      ROS_INFO("Encoder requires Enable. Enabling inputs....\n");
      for (i = 0 ; i < inputcount ; i++)
	CPhidgetEncoder_setEnabled((CPhidgetEncoderHandle)ENC, i, 1);
    }
  return 0;
}

//callback that will run if the Spatial is detached from the computer
int CCONV DetachHandler(CPhidgetHandle spatial, void *userptr){

  int serialNo;
  CPhidget_getSerialNumber(spatial, &serialNo);
  ROS_INFO("Phidget Encoder %10d detached!", serialNo);
  
  return 0;
}

//callback that will run if the Spatial generates an error
int CCONV ErrorHandler(CPhidgetHandle handle, void *userptr, int ErrorCode, const char *unknown)
{
  ROS_ERROR("Error handled. %d - %s", ErrorCode, unknown);
  return 0;
}

/*************************************************************
/
/            Phidget Encoder Class
/
/*************************************************************/
void PhidgetENC::poseCallback(const sensor_msgs::Imu& imu_data)
{
  int lf, lb, rf, rb;//Left front, left back, right front, right back

  geometry_msgs::QuaternionStamped qs;
  qs.quaternion = imu_data.orientation;
  qs.header.frame_id = imu_data.header.frame_id;
  trans.transformQuaternion(enc_odom.header.frame_id, qs, qs);
  // use tf built-in function for yaw calculation
  double yaw = tf::getYaw(qs.quaternion);

  ros::Time oldtime = enc_odom.header.stamp;
  enc_odom.header.stamp = ros::Time::now();
  
  // Read data from encoders
  CPhidgetEncoder_getPosition(enc_handle, LF, &lf);
  CPhidgetEncoder_getPosition(enc_handle, RF, &rf);
  // Zero them
  CPhidgetEncoder_setPosition(enc_handle, LF, 0);
  CPhidgetEncoder_setPosition(enc_handle, RF, 0);

  // Update pose
  
  double delT = (enc_odom.header.stamp-oldtime).toSec();
  // TODO: which side shall I scale?
  double vl   = -lf * MeterPerSecPerTick / delT * DIAMETER_RESCALE;
  double vr   =  rf * MeterPerSecPerTick / delT;
  double LinV = (vr + vl) * 0.5;
  double AngV = (vr - vl) * 0.5;
  double vx = LinV * cos(yaw);
  double vy = LinV * sin(yaw);

  enc_odom.pose.pose.position.x += vx * delT;
  enc_odom.pose.pose.position.y += vy * delT;
  enc_odom.pose.pose.position.z += 0.0;

  enc_odom.pose.pose.orientation = qs.quaternion;
  
  // Linear and angular velocities
  enc_odom.twist.twist.linear.x = vx;
  enc_odom.twist.twist.linear.y = vy;
  enc_odom.twist.twist.angular.z = AngV;

  odom_pub.publish(enc_odom);

  // publish ground speed
  geometry_msgs::Vector3 groundSpeed;
  groundSpeed.x = vl;
  groundSpeed.y = vr;
  gs_pub.publish(groundSpeed);
}

/*************************************************
/                 Constructor
*************************************************/
PhidgetENC::PhidgetENC(ros::NodeHandle& n, ros::NodeHandle& p)
  :enc_handle(0),nh(n), pn(p),trans(ros::Duration(1))
{
  init_phidget_enc();

  init_ros_enc();
  //  config_enc();
}

PhidgetENC::~PhidgetENC()
{
  CPhidget_close( (CPhidgetHandle)enc_handle);
  CPhidget_delete((CPhidgetHandle)enc_handle);
}

void PhidgetENC::init_phidget_enc(void){
   //create the spatial object
    CPhidgetEncoder_create(&enc_handle);

    //Set the handlers to be run when the device is:
    //-> plugged in or opened from software, 
    //-> unplugged or closed from software,
    //-> generates an error.
    CPhidget_set_OnAttach_Handler((CPhidgetHandle)enc_handle, ENC_AttachHandler, NULL);
    CPhidget_set_OnDetach_Handler((CPhidgetHandle)enc_handle, DetachHandler,     NULL);
    CPhidget_set_OnError_Handler( (CPhidgetHandle)enc_handle, ErrorHandler,      NULL);

    //open the spatial object for device connections
    CPhidget_open((CPhidgetHandle)enc_handle, -1);

    ROS_INFO("Waiting for spatial to be attached.... \n");

    int result = CPhidget_waitForAttachment((CPhidgetHandle)enc_handle, 10000);
    const char* err;
    CPhidget_getErrorDescription(result, &err);
    ROS_ERROR("Problem waiting for attachment: %s\n", err);
}

void PhidgetENC::init_ros_enc(void){
  //Configure ROS publisher
  odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);
  gs_pub   = nh.advertise<geometry_msgs::Vector3>("ground_speed", 10);

  pose_sub = nh.subscribe("imu_data", 10, &PhidgetENC::poseCallback, this);

  std::string frameid_;
  pn.param("ENC_frameID", frameid_, std::string("base_link"));

  enc_odom.header.frame_id = frameid_;

  // Initialize position
  enc_odom.pose.pose.position.x = 0.0;
  enc_odom.pose.pose.position.y = 0.0;
  enc_odom.pose.pose.position.z = 0.0;

  // Orientation
  enc_odom.pose.pose.orientation.x = 0.0;
  enc_odom.pose.pose.orientation.y = 0.0;
  enc_odom.pose.pose.orientation.z = 0.0;
  enc_odom.pose.pose.orientation.w = 1.0;

  // For now just assume static covariance which is apparently incorrect
  for(int i = 0; i < 6; i++)
    enc_odom.pose.covariance[i*6+i] = 1;

  // Linear velocities
  enc_odom.twist.twist.linear.x = 0.0;
  enc_odom.twist.twist.linear.y = 0.0;
  enc_odom.twist.twist.linear.z = 0.0;
  // Angular velocities
  enc_odom.twist.twist.angular.x = 0.0;
  enc_odom.twist.twist.angular.y = 0.0;
  enc_odom.twist.twist.angular.z = 0.0;
  // Twist covariance
  for(int i = 0; i < 3; i++)
    enc_odom.twist.covariance[6*i+i] = 1;
  for(int i = 3; i < 6; i++)
    enc_odom.twist.covariance[6*i+i] = 100;
}

//void PhidgetENC::config_enc(){}

/*******************************************
/              Main program
*******************************************/
int main(int argc, char* argv[])
{
  // Initialize ROS
  ros::init(argc, argv, "encoder_node");
  ros::NodeHandle pn("~");
  ros::NodeHandle n;

  PhidgetENC enc(n, pn);

  // must do this to enable the callbacks
  ros::spin();

  ROS_INFO("Encoder node exits");
}
